import numpy as np
import math
import cv2
def euler_to_rotation_matrix(rx, ry, rz):
    """
    Convert Euler angles (in radians) to a 3x3 rotation matrix.
    """
    rx = rx
    ry = ry
    rz = rz
    R_x = np.array([[1, 0, 0],
                    [0, math.cos(rx), -math.sin(rx)],
                    [0, math.sin(rx), math.cos(rx)]])

    R_y = np.array([[math.cos(ry), 0, math.sin(ry)],
                    [0, 1, 0],
                    [-math.sin(ry), 0, math.cos(ry)]])

    R_z = np.array([[math.cos(rz), -math.sin(rz), 0],
                    [math.sin(rz), math.cos(rz), 0],
                    [0, 0, 1]])

    R = np.dot(R_z, np.dot(R_y, R_x))
    return R

def pose_to_matrix(pose):
    """
    Convert pose information [x, y, z, rx, ry, rz] to a 4x4 transformation matrix.
    """
    x, y, z, rx, ry, rz = pose
    translation = np.array([[x], [y], [z]])
    rotation = cv2.Rodrigues(np.array([rx, ry, rz]))[0]
    matrix = np.vstack((np.hstack((rotation, translation)), [0, 0, 0, 1]))
    return matrix

def multiply_transforms(ini_pose, trans_pose):
    """
    Multiply two transformation matrices obtained from initial and transformation poses.
    """
    T_ini = pose_to_matrix(ini_pose)
    T_trans = pose_to_matrix(trans_pose)
    T_final = np.dot(T_ini, T_trans)
    return T_final

def matrix_to_pose(matrix):
    """
    Convert a 4x4 transformation matrix to pose information [x, y, z, rx, ry, rz].
    """
    translation = matrix[:3, 3]
    rotation = matrix[:3, :3]
    R_vect = cv2.Rodrigues(rotation)[0]
    pose = np.array([translation[0], translation[1], translation[2], R_vect[0, 0], R_vect[1, 0], R_vect[2, 0]])
    return pose

# Example usage:
end_pose = [50.39, -532.55, 636.77, -179.28/180*np.pi, 1.09/180*np.pi, -130.18/180*np.pi] #末端位姿
trans_pose_e2c = [-36.7699, 12.7280, 95.5, -0.004298208386645902, -0.00835105847834975, -np.pi/4 + 0.0029265518691546388]#末端到相机，这个测得或者标定出来
trans_pose_c2b = [-15,37,439,0.0001,0.0001,0.0001]#相机到螺栓，这个由程序返回
trans_pose_e2h = [45.96,45.96,269,0.0001,0.0001,0.0001]#末端到手，可以量出
camera_matrix = multiply_transforms(end_pose, trans_pose_e2c)
camera_pose = matrix_to_pose(camera_matrix)
bolt_pose = multiply_transforms(camera_pose, trans_pose_c2b)
hand_pose = multiply_transforms(end_pose, trans_pose_e2h)
result_pose = matrix_to_pose(bolt_pose) - matrix_to_pose(hand_pose) + end_pose#最终的末端位姿
print("Final Pose:", result_pose)
